
// import haxe.ds.Array;
// import oimo.collision.narrowphase.*;
// import oimo.common.Setting;
// import oimo.common.Transform;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Manifold } from "./Manifold";
import { ManifoldPoint } from "./ManifoldPoint";
import { Setting } from "../../../common/Setting";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { DetectorResultPoint } from "../../../collision/narrowphase/DetectorResultPoint";
import { Transform } from "../../../common/Transform";
import { DetectorResult } from "../../../collision/narrowphase/DetectorResult";

/**
 * numberernal class
 */


export class ManifoldUpdater {
	_manifold: Manifold;

	numOldPoints: number;
	oldPoints: Array<ManifoldPoint>;

	constructor(manifold: Manifold) {
		this._manifold = manifold;

		this.numOldPoints = 0;
		this.oldPoints = new Array<ManifoldPoint>(Setting.maxManifoldPoints);
		for (let i = 0; i < Setting.maxManifoldPoints; ++i) {
			this.oldPoints[i] = new ManifoldPoint();
		}
	}

	// --- ---

	public removeOutdatedPoints(): void {
		var num: number = this._manifold._numPoints;
		var index: number = num;
		while (--index >= 0) {
			var p: ManifoldPoint = this._manifold._points[index];

			var diff: Vec3 = new Vec3();
			M.vec3_sub(diff, p._pos1, p._pos2);
			var dotN: number = M.vec3_dot(this._manifold._normal, diff);

			if (dotN > Setting.contactPersistenceThreshold) {
				this.removeManifoldPoint(index);
				continue;
			}

			// compute projection of diff
			diff.addScaledEq(this._manifold._normal, -dotN);
			if (M.vec3_dot(diff, diff) > Setting.contactPersistenceThreshold * Setting.contactPersistenceThreshold) {
				// the amount of horizontal sliding exceeds threshold
				this.removeManifoldPoint(index);
				continue;
			}
		}
	}

	public removeManifoldPoint(index: number): void {
		var lastIndex: number = --this._manifold._numPoints;
		if (index != lastIndex) {
			var tmp: ManifoldPoint = this._manifold._points[index];
			this._manifold._points[index] = this._manifold._points[lastIndex];
			this._manifold._points[lastIndex] = tmp;
		}
		this._manifold._points[lastIndex]._clear();
	}

	public addManifoldPoint(point: DetectorResultPoint, tf1: Transform, tf2: Transform): void {
		// check if the number of points will exceed the limit
		var num: number = this._manifold._numPoints;
		if (num == Setting.maxManifoldPoints) {
			var targetIndex: number = this.computeTargetIndex(point, tf1, tf2);
			this._manifold._points[targetIndex]._initialize(point, tf1, tf2);
			return;
		}

		// just add the point
		this._manifold._points[num]._initialize(point, tf1, tf2);
		this._manifold._numPoints++;
	}

	public computeTargetIndex(newPoint: DetectorResultPoint, tf1: Transform, tf2: Transform): number {
		var p1: ManifoldPoint = this._manifold._points[0];
		var p2: ManifoldPoint = this._manifold._points[1];
		var p3: ManifoldPoint = this._manifold._points[2];
		var p4: ManifoldPoint = this._manifold._points[3];
		var maxDepth: number = p1._depth;
		var maxDepthIndex: number = 0;
		if (p2._depth > maxDepth) {
			maxDepth = p2._depth;
			maxDepthIndex = 1;
		}
		if (p3._depth > maxDepth) {
			maxDepth = p3._depth;
			maxDepthIndex = 2;
		}
		if (p4._depth > maxDepth) {
			maxDepth = p4._depth;
			maxDepthIndex = 3;
		}

		var rp1: Vec3 = newPoint.position1.clone();
		M.vec3_sub(rp1, rp1, tf1._position);

		var a1: number = (this.quadAreaFast(p2._relPos1, p3._relPos1, p4._relPos1, rp1));
		var a2: number = (this.quadAreaFast(p1._relPos1, p3._relPos1, p4._relPos1, rp1));
		var a3: number = (this.quadAreaFast(p1._relPos1, p2._relPos1, p4._relPos1, rp1));
		var a4: number = (this.quadAreaFast(p1._relPos1, p2._relPos1, p3._relPos1, rp1));
		var max: number = a1;
		var target: number = 0;
		if (a2 > max && maxDepthIndex != 1 || maxDepthIndex == 0) {
			max = a2;
			target = 1;
		}
		if (a3 > max && maxDepthIndex != 2) {
			max = a3;
			target = 2;
		}
		if (a4 > max && maxDepthIndex != 3) {
			max = a4;
			target = 3;
		}
		return target;
	}


	private quadAreaFast(p1: Vec3, p2: Vec3, p3: Vec3, p4: Vec3): number {
		// possible diagonals (12-34, 13-24, 14-23)
		var v12: Vec3 = new Vec3();
		var v34: Vec3 = new Vec3();
		var v13: Vec3 = new Vec3();
		var v24: Vec3 = new Vec3();
		var v14: Vec3 = new Vec3();
		var v23: Vec3 = new Vec3();
		M.vec3_sub(v12, p2, p1);
		M.vec3_sub(v34, p4, p3);
		M.vec3_sub(v13, p3, p1);
		M.vec3_sub(v24, p4, p2);
		M.vec3_sub(v14, p4, p1);
		M.vec3_sub(v23, p3, p2);
		var cross1: Vec3 = v12.crossEq(v34);
		var cross2: Vec3 = v13.crossEq(v24);
		var cross3: Vec3 = v14.crossEq(v23);;
		var a1: number = M.vec3_dot(cross1, cross1);
		var a2: number = M.vec3_dot(cross2, cross2);
		var a3: number = M.vec3_dot(cross3, cross3);
		if (a1 > a2) {
			if (a1 > a3) {
				return a1;
			} else {
				return a3;
			}
		} else {
			if (a2 > a3) {
				return a2;
			} else {
				return a3;
			}
		}
	}

	public computeRelativePositions(tf1: Transform, tf2: Transform): void {
		var num: number = this._manifold._numPoints;
		for (let i = 0; i < num; ++i) {
			var p: ManifoldPoint = this._manifold._points[i];
			M.vec3_mulMat3(p._relPos1, p._localPos1, tf1._rotation);
			M.vec3_mulMat3(p._relPos2, p._localPos2, tf2._rotation);
			p._warmStarted = true; // set warm starting flag
		}
	}

	public findNearestContactPointIndex(target: DetectorResultPoint, tf1: Transform, tf2: Transform): number {
		var nearestSq: number = Setting.contactPersistenceThreshold * Setting.contactPersistenceThreshold;
		var idx: number = -1;
		for (let i = 0; i < this._manifold._numPoints; ++i) {
			var d: number = this.distSq(this._manifold._points[i], target, tf1, tf2);
			//trace("d is " + d);
			if (d < nearestSq) {
				nearestSq = d;
				idx = i;
			}
		}
		//trace("idx is " + idx);
		return idx;
	}


	private distSq(mp: ManifoldPoint, result: DetectorResultPoint, tf1: Transform, tf2: Transform): number {
		var rp1: Vec3 = result.position1.clone();
		var rp2: Vec3 = result.position2.clone();
		M.vec3_sub(rp1, rp1, tf1._position);
		M.vec3_sub(rp2, rp2, tf2._position);

		var diff1: Vec3 = new Vec3();
		var diff2: Vec3 = new Vec3();
		M.vec3_sub(diff1, mp._relPos1, rp1);
		M.vec3_sub(diff2, mp._relPos2, rp2);

		var sq1: number = M.vec3_dot(diff1, diff1);
		var sq2: number = M.vec3_dot(diff2, diff2);
		//trace("sq1: " + sq1 + ", sq2: " + sq2);
		return sq1 < sq2 ? sq1 : sq2;
	}


	private saveOldData(): void {
		this.numOldPoints = this._manifold._numPoints;
		for (let i = 0; i < this.numOldPoints; ++i) {
			this.oldPoints[i]._copyFrom(this._manifold._points[i]);
		}
	}


	private updateContactPointById(cp: ManifoldPoint): void {
		for (let i = 0; i < this.numOldPoints; ++i) {
			var ocp: ManifoldPoint = this.oldPoints[i];
			if (cp._id == ocp._id) {
				cp._impulse.copyFrom(ocp._impulse);
				cp._warmStarted = true;
				break;
			}
		}
	}

	// --- internal ---

	public totalUpdate(result: DetectorResult, tf1: Transform, tf2: Transform): void {
		this.saveOldData();

		var num: number = result.numPoints;
		this._manifold._numPoints = num;
		for (let i = 0; i < num; ++i) {
			var p: ManifoldPoint = this._manifold._points[i];
			var ref: DetectorResultPoint = result.points[i];
			p._initialize(ref, tf1, tf2);
			this.updateContactPointById(p);
		}
	}

	public incrementalUpdate(result: DetectorResult, tf1: Transform, tf2: Transform): void {
		// update old data
		this._manifold._updateDepthsAndPositions(tf1, tf2);

		// set warm started flag
		for (let i = 0; i < this._manifold._numPoints; ++i) {
			this._manifold._points[i]._warmStarted = true;
		}

		M.assert(result.numPoints == 1);
		var newPoint: DetectorResultPoint = result.points[0];

		// add or update point
		var index: number = this.findNearestContactPointIndex(newPoint, tf1, tf2);
		if (index == -1) {
			this.addManifoldPoint(newPoint, tf1, tf2);
		} else {
			var cp: ManifoldPoint = this._manifold._points[index];
			cp._updateDepthAndPositions(newPoint, tf1, tf2);
		}

		// remove some points
		this.removeOutdatedPoints();
	}
}
